/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package Accelerometer;

import Communication.Communicator;
import Wasa.*;
import java.util.ArrayList;
import java.util.logging.Level;
import java.util.logging.Logger;

/**
 *
 * @author gianmariospacagna
 */
public class Accelerometer {

    /**
     * @param args the command line arguments
     */
    Acceleration currentAcc;
    Velocity currentVel;
    Position currentPos;
    int size;
    private Communicator myCommunicator;
    RealTimeMeasurement rtm;
    /*
     * 1=EarPiece
     * 2=SpeakerPhone
     */
    int speakersStatus;

    private Accelerometer(Communicator myCommunicator) {
    }

    private Accelerometer(String port, String size) throws WrongWASADataFormatException {


        this.myCommunicator = new Communicator(port);
        System.out.println("Communicator initialized");
        this.size = Integer.parseInt(size);
        currentAcc = new Acceleration(-1, 0, 0, 0);
        currentVel = new Velocity(currentAcc);
        currentPos = new Position(currentVel);


        run();



    }

    private void run() throws WrongWASADataFormatException {

        System.out.println("Application running");

        /*Stream measurements
         * AccStreamMeasurement asm=new AccStreamMeasurement(myCommunicator, 32);
        
        asm.start();
        
        System.out.println("Acc Stream Measurement Thread started");
         */



        /*Evaluate latency
         * 
         * System.out.println("Latency evaluation [ms]");
        evaluateLatency(1000);
        
         */


        /*Rel Time only acceleration
        
        System.out.println("Real Time Only Acc measurements");
        rtAccMeasurements();*/


        /*Real time position*/
        System.out.println("Real Time measurements on X Y : acc\tvel\tpos");
        rtMeasurements();

        //rtMeasurementsNoise();


    }

    void activateMeasurements() {
        rtm.activate();
    }

    void standbyeMeasurements() {
        rtm.standbye();
    }

    public static void main(String[] args) throws WrongWASADataFormatException {
        // TODO code application logic here

        new Accelerometer(args[0], args[1]);


    }

    private void evaluateLatency(int n) throws WrongWASADataFormatException {
        int i = 0;
        long[] times = new long[n];
        while (i < n) {
            long initTime = System.currentTimeMillis();
            Acceleration a = myCommunicator.measureAcceleration();
            //System.out.println("Acceleration measured: " + a.toString());
            long deltaT = a.getTime() - initTime;
            //System.out.println("Delta Time: "+deltaT);

            times[i++] = deltaT;
        }

        float mean = mean(times);
        float var = variance(times, mean);
        float stdev = (float) Math.sqrt(var);

        System.out.println("Mean: " + mean + "\tVar: " + var + "\tStDev: " + stdev);


    }

    public float variance(long[] times, float mean) {
        if (times.length <= 1) {
            return 0;
        }
        float var = 0;
        for (long t : times) {
            var += ((t - mean) * (t - mean));



        }
        return var /= times.length - 1;
    }

    private float mean(long[] times) {
        float mean = 0;
        for (long t : times) {
            mean += t;
        }
        return mean /= times.length;
    }

    private void rtAccMeasurements() {
        while (true) {
            Acceleration a;
            try {
                a = myCommunicator.measureAcceleration();
                System.out.println(a.toStringTXYZ());
            } catch (WrongWASADataFormatException ex) {
                Logger.getLogger(Accelerometer.class.getName()).log(Level.SEVERE, null, ex);
            }

        }
    }

    private void rtMeasurements() {
        ArrayList<Acceleration> accMeasures = new ArrayList<Acceleration>();
        ArrayList<Velocity> velMeasures = new ArrayList<Velocity>();
        ArrayList<Position> posMeasures = new ArrayList<Position>();

        //Acceleration rtAcc = myCommunicator.measureAcceleration();



        //Velocity rtVel = new Velocity(rtAcc);

        //Position rtPos = new Position(rtVel);

        boolean done = false;

        int n = 0;
        while (n < 1) {
            int k = 1;
            System.out.println("Round " + n);
            Acceleration rtAcc = new Acceleration();
            Velocity rtVel = new Velocity();
            Position rtPos = new Position();
            Acceleration initAcc = new Acceleration();
            boolean calibrated = false;
            while (!calibrated) {
                try {
                    initAcc = calibrateAcc(100);
                    calibrated = true;
                } catch (WrongWASADataFormatException ex) {
                    Logger.getLogger(Accelerometer.class.getName()).log(Level.SEVERE, null, ex);
                    System.out.println("Restart calibration procedure");
                }
            }

            //Acceleration rtAcc = new Acceleration(initAcc.getTime(), initAcc.getX(), initAcc.getY(), initAcc.getZ());

            System.out.println("Initial Acceleration: " + initAcc.toStringTXYZ());
            System.out.println("###NOW YOU CAN MOVE YOUR WASA###");
            while (rtAcc.getTime() - initAcc.getTime() < 100000000) {

                try {
                    rtAcc = myCommunicator.measureAcceleration();
                    rtAcc.normalize(initAcc);


                } catch (WrongWASADataFormatException ex) {
                    //Logger.getLogger(Accelerometer.class.getName()).log(Level.SEVERE, null, ex);
                }
                rtVel.updateVelocity(rtAcc);
                rtPos.updatePosition(rtVel);
                //System.out.println((rtAcc.getX()-initAcc.getX())+" ; "+(rtAcc.getY()-initAcc.getY()));
                //double[] m = m(accMeasures);
                //System.out.println((m[0]-initAcc.getX())+"\t"+(m[1]-initAcc.getY()));
            /*System.out.println(rtAcc.getX() + "\t" + rtAcc.getY() + "\t"
                + rtVel.getX() + "\t" + rtVel.getY() + "\t"
                + rtPos.getX() + "\t" + rtPos.getY());*/



                System.out.println("Timestamp[ms] T" + (k) + "=" + rtAcc.getTime());

                System.out.println("Accx:" + rtAcc.getX() + "\t\t\t\tAccy:" + rtAcc.getY());

                System.out.println("Velx:" + rtVel.getX() + "\t\t\t\tVely:" + rtVel.getY());

                System.out.println("Posx:" + rtPos.getX() + "\t\t\t\tPosy:" + rtPos.getY());

                System.out.println("-----------------------");


                k++;

            }

            posMeasures.add(rtPos);
            n++;


            System.out.println("Next Round in 3 seconds");
            try {
                Thread.sleep(3000);
            } catch (InterruptedException ex) {
                Logger.getLogger(Accelerometer.class.getName()).log(Level.SEVERE, null, ex);
            }
        }
    }

    private void rtMeasurementsNoise() {
        ArrayList<Acceleration> accMeasures = new ArrayList<Acceleration>();
        ArrayList<Velocity> velMeasures = new ArrayList<Velocity>();
        ArrayList<Position> posMeasures = new ArrayList<Position>();


        int n = 0;
        while (n < 1) {
            int k = 0;
            System.out.println("Round " + n);
            ArrayList<Acceleration> noise = new ArrayList<Acceleration>();;
            Acceleration rtAcc = new Acceleration();
            Velocity rtVel = new Velocity();
            Position rtPos = new Position();
            Acceleration initAcc = new Acceleration();
            
            
            
            boolean noiseEvaluated = false;
            while (!noiseEvaluated) {
                try {
                    noise = evaluateNoise(1000);
                    noiseEvaluated = true;
                } catch (WrongWASADataFormatException ex) {
                    Logger.getLogger(Accelerometer.class.getName()).log(Level.SEVERE, null, ex);
                    System.out.println("Restart noise eval procedure");

                }
            }

            int t = 5;
            while (t > 0) {
                try {
                    System.out.println("Wait " + t + " secs the Wasa cools down");
                    Thread.sleep(1000);
                    t--;
                } catch (InterruptedException ex) {
                    Logger.getLogger(Accelerometer.class.getName()).log(Level.SEVERE, null, ex);
                }
            }

            //Acceleration rtAcc = new Acceleration(initAcc.getTime(), initAcc.getX(), initAcc.getY(), initAcc.getZ());

            //System.out.println("Initial Acceleration: " + initAcc.toStringTXYZ());
            System.out.println("###NOW YOU CAN MOVE YOUR WASA###");
            while (k < 1000) {

                try {
                    rtAcc = myCommunicator.measureAcceleration();
                    rtAcc.normalize(noise.get(k ));


                } catch (WrongWASADataFormatException ex) {
                    //Logger.getLogger(Accelerometer.class.getName()).log(Level.SEVERE, null, ex);
                }
                rtVel.updateVelocity(rtAcc);
                rtPos.updatePosition(rtVel);
                //System.out.println((rtAcc.getX()-initAcc.getX())+" ; "+(rtAcc.getY()-initAcc.getY()));
                //double[] m = m(accMeasures);
                //System.out.println((m[0]-initAcc.getX())+"\t"+(m[1]-initAcc.getY()));
            /*System.out.println(rtAcc.getX() + "\t" + rtAcc.getY() + "\t"
                + rtVel.getX() + "\t" + rtVel.getY() + "\t"
                + rtPos.getX() + "\t" + rtPos.getY());*/



                System.out.println("Timestamp[ms] T" + (k) + "=" + rtAcc.getTime());

                System.out.println("Accx:" + rtAcc.getX() + "\t\t\t\tAccy:" + rtAcc.getY());

                System.out.println("Velx:" + rtVel.getX() + "\t\t\t\tVely:" + rtVel.getY());

                System.out.println("Posx:" + rtPos.getX() + "\t\t\t\tPosy:" + rtPos.getY());

                System.out.println("-----------------------");


                k++;

            }

            posMeasures.add(rtPos);
            n++;


            System.out.println("Next Round in 3 seconds");
            try {
                Thread.sleep(3000);
            } catch (InterruptedException ex) {
                Logger.getLogger(Accelerometer.class.getName()).log(Level.SEVERE, null, ex);
            }
        }



        double[] meanPos = meanPos(posMeasures);
        double[] stDevPos = stDevPos(posMeasures, meanPos);


        System.out.println("MeanX:" + meanPos[0] + "\tStDevX:" + stDevPos[0]);
        System.out.println("MeanY:" + meanPos[1] + "\tStDevY:" + stDevPos[1]);


    }

    private double[] meanAcc(ArrayList<Acceleration> accMeasures) {
        double[] mean = new double[3];
        mean[0] = 0;
        mean[1] = 0;
        mean[2] = 0;
        for (Acceleration a : accMeasures) {
            mean[0] += a.getX();
            mean[1] += a.getY();
            mean[2] += a.getZ();
        }
        int n = accMeasures.size();
        mean[0] /= n;
        mean[1] /= n;
        mean[2] /= n;
        return mean;
    }

    private double[] meanPos(ArrayList<Position> posMeasures) {
        double[] m = new double[3];
        m[0] = 0;
        m[1] = 0;
        m[2] = 0;
        for (Position a : posMeasures) {
            m[0] += a.getX();
            m[1] += a.getY();
            m[2] += a.getZ();
        }
        int n = posMeasures.size();
        m[0] /= n;
        m[1] /= n;
        m[2] /= n;
        return m;
    }

    private double[] stDevPos(ArrayList<Position> posMeasures, double[] mean) {
        int n = posMeasures.size();
        if (n < 2) {
            return new double[]{0, 0, 0};
        }
        double[] stDev = new double[3];
        for (Position p : posMeasures) {
            stDev[0] += Math.pow(p.getX() - mean[0], 2);
            stDev[1] += Math.pow(p.getY() - mean[0], 2);
            stDev[2] += Math.pow(p.getZ() - mean[0], 2);

        }

        stDev[0] /= n - 1;
        stDev[1] /= n - 1;
        stDev[2] /= n - 2;

        stDev[0] = Math.sqrt(stDev[0]);
        stDev[1] = Math.sqrt(stDev[1]);
        stDev[2] = Math.sqrt(stDev[2]);
        return stDev;
    }

    private Acceleration calibrateAcc(int n) throws WrongWASADataFormatException {
        System.out.println("Initial Acceleration calibration... pls don't move the WASA");
        //ArrayList<Acceleration> accSamples = new ArrayList<Acceleration>();
        double x = 0;
        double y = 0;
        double z = 0;
        for (int i = 0; i < n; i++) {
            Acceleration a = myCommunicator.measureAcceleration();
            if (a == null) {
                i--;
                break;
            }
            //System.out.println(a.toStringTXYZ());
            x += a.getX();
            y += a.getY();
            z += a.getZ();
        }


        x = x / n;
        y = y / n;
        z = z / n;
        return new Acceleration(System.currentTimeMillis(), x, y, z);
    }

    private ArrayList<Acceleration> evaluateNoise(int n) throws WrongWASADataFormatException {
        System.out.println("Noise evaluation "+n+" samples... pls don't move the WASA");

        ArrayList<Acceleration> noise = new ArrayList<Acceleration>();
        for (int i = 0; i < n; i++) {
            Acceleration a;
            a = myCommunicator.measureAcceleration();
            noise.add(a);
            System.out.print(i);
            //System.out.println(a.toStringTXYZ());

        }

        return noise;

    }
}
